﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using TwinCAT.Ads;

namespace SCARA_GUI
{
    class SCARAComm
    {
        private TcAdsClient _tcClient;
        private AdsStream adsWriteStream;
        private AdsStream adsReadStream;
        public int clientPort;

        public SCARAComm()
        {
            adsWriteStream = new AdsStream(1);
            adsReadStream = new AdsStream(1);
            _tcClient = new TcAdsClient();
        }

        public bool Connect(string NetId, Int32 port)
        {
            try
            {
                _tcClient.Connect(NetId, port);
                clientPort = _tcClient.ClientAddress.Port;
            }
            catch
            {
                return false;
            }
            return true;
        }

        public void EnableDrive(Byte motorNumber)
        {
            try
            {
                AdsBinaryWriter binWriter = new AdsBinaryWriter(adsWriteStream);
                adsWriteStream.Position = 0;
                motorNumber -= 1;
                binWriter.Write(motorNumber);
                _tcClient.ReadWrite((UInt32)0x2, 0x1, adsReadStream, adsWriteStream);
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public void DisableDrive(Byte motorNumber)
        {
            try
            {
                AdsBinaryWriter binWriter = new AdsBinaryWriter(adsWriteStream);
                adsWriteStream.Position = 0;
                motorNumber -= 1;
                binWriter.Write(motorNumber);
                _tcClient.ReadWrite((UInt32)0x2, 0x2, adsReadStream, adsWriteStream);
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public void ClearDriveFault(Byte motorNumber)
        {
            try
            {
                AdsBinaryWriter binWriter = new AdsBinaryWriter(adsWriteStream);
                adsWriteStream.Position = 0;
                motorNumber -= 1;
                binWriter.Write(motorNumber);
                _tcClient.ReadWrite((UInt32)0x2, 0x3, adsReadStream, adsWriteStream);
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public void MoveDrive(Byte motorNumber, bool isRelative, double pos, double vel, double acc)
        {
            try
            {
                AdsStream writeStream = new AdsStream(3 * 8 + 2);
                AdsBinaryWriter binWriter = new AdsBinaryWriter(writeStream);
                writeStream.Position = 0;
                motorNumber -= 1;
                binWriter.Write(motorNumber);
                binWriter.Write(isRelative);//Relative Move
                binWriter.Write(pos);//Position, absolute or relative
                binWriter.Write(vel);//Speed
                binWriter.Write(acc);//Acceleration
                _tcClient.ReadWrite((UInt32)0x2, 0x4, adsReadStream, writeStream);
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public void StopDrive(Byte motorNumber)
        {
            try
            {
                AdsBinaryWriter binWriter = new AdsBinaryWriter(adsWriteStream);
                adsWriteStream.Position = 0;
                motorNumber -= 1;
                binWriter.Write(motorNumber);
                _tcClient.ReadWrite((UInt32)0x2, 0x5, adsReadStream, adsWriteStream);
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public string[] GetDriveState()
        {
            AdsStream readStream = new AdsStream(4);
            string[] state = new string[4];

            try
            {
                _tcClient.ReadWrite((UInt32)0x1, 0x1, readStream, adsWriteStream);
                byte[] stateBuffer = readStream.ToArray();
                for (int i = 0; i < 4; i++)
                {
                    switch (stateBuffer[i])
                    {
                        case 0:
                            state[i] = "NOT_READY";
                            break;
                        case 1:
                            state[i] = "CSP_READY";
                            break;
                        case 2:
                            state[i] = "CST_READY";
                            break;
                        case 3:
                            state[i] = "DRIVE_FAULT";
                            break;
                        case 4:
                            state[i] = "POSITION_OUT_OF_LIMIT";
                            break;
                        default:
                            break;
                    }
                }
                return state;
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public string[] GetProfileState()
        {
            AdsStream readStream = new AdsStream(4);
            string[] state = new string[4];

            try
            {
                _tcClient.ReadWrite((UInt32)0x1, 0x5, readStream, adsWriteStream);
                byte[] stateBuffer = readStream.ToArray();
                for (int i = 0; i < 4; i++)
                {
                    switch (stateBuffer[i])
                    {
                        case 0:
                            state[i] = "NO_PROFILE";
                            break;
                        case 1:
                            state[i] = "TVP_PACC";
                            break;
                        case 2:
                            state[i] = "TVP_PCV";
                            break;
                        case 3:
                            state[i] = "TVP_PDEACC";
                            break;
                        case 4:
                            state[i] = "TVP_NACC";
                            break;
                        case 5:
                            state[i] = "TVP_NCV";
                            break;
                        case 6:
                            state[i] = "TVP_NDEACC";
                            break;
                        case 7:
                            state[i] = "TVP_PPREDEACC";
                            break;
                        case 8:
                            state[i] = "TVP_FINISH";
                            break;
                        case 9:
                            state[i] = "PGP_START";
                            break;
                        case 10:
                            state[i] = "PGP_EXEC";
                            break;
                        case 11:
                            state[i] = "PGP_FINISH";
                            break;
                        case 12:
                            state[i] = "PGP_REVERSE_START";
                            break;
                        case 13:
                            state[i] = "PGP_REVERSE";
                            break;
                        default:
                            break;
                    }
                }
                return state;
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public double[] GetDrivePos()
        {
            AdsStream readStream = new AdsStream(4 * 8);
            double[] pos = new double[4];

            try
            {
                _tcClient.ReadWrite((UInt32)0x1, 0x2, readStream, adsWriteStream);
                byte[] posBuffer = readStream.ToArray();
                for (int i = 0; i < 4; i++)
                {
                    pos[i] = BitConverter.ToDouble(posBuffer, 8 * i);
                }
                return pos;
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public double[] GetDriveVel()
        {
            AdsStream readStream = new AdsStream(4 * 8);
            double[] vel = new double[4];

            try
            {
                _tcClient.ReadWrite((UInt32)0x1, 0x3, readStream, adsWriteStream);
                byte[] velBuffer = readStream.ToArray();
                for (int i = 0; i < 4; i++)
                {
                    vel[i] = BitConverter.ToDouble(velBuffer, 8 * i);
                }
                return vel;
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public double[] GetDriveTq()
        {
            AdsStream readStream = new AdsStream(4 * 8);
            double[] tq = new double[4];

            try
            {
                _tcClient.ReadWrite((UInt32)0x1, 0x4, readStream, adsWriteStream);
                byte[] tqBuffer = readStream.ToArray();
                for (int i = 0; i < 4; i++)
                {
                    tq[i] = BitConverter.ToDouble(tqBuffer, 8 * i);
                }
                return tq;
            }
            catch (Exception err)
            {
                throw (err);
            }
        }

        public void InputTrajectory(Byte motorNumber, double[] trajectory)
        {
            try
            {
                int length = trajectory.Length;

                AdsStream writeStream = new AdsStream(1 + 4 + length * 8);
                AdsBinaryWriter binWriter = new AdsBinaryWriter(writeStream);
                writeStream.Position = 0;

                motorNumber -= 1;
                binWriter.Write(motorNumber);
                binWriter.Write(length);
                for (int i = 0; i < length; i++)
                    binWriter.Write(trajectory[i]);

                _tcClient.ReadWrite((UInt32)0x2, 0x6, adsReadStream, writeStream);

            }
            catch (Exception err)
            {
                throw err;
            }
        }

        public void StartExecution(bool isLoop)
        {
            try
            {
                AdsStream writeStream = new AdsStream(1);
                AdsBinaryWriter binWriter = new AdsBinaryWriter(writeStream);
                writeStream.Position = 0;

                binWriter.Write(isLoop);
                _tcClient.ReadWrite((UInt32)0x02, 0x7, adsReadStream, writeStream);
            }
            catch (Exception err)
            {
                throw (err);
            }
        }
        public void AbortExecution()
        {
            try
            {
                StopDrive(1);
                StopDrive(2);
                StopDrive(3);
                StopDrive(4);
            }
            catch (Exception err)
            {
                throw (err);
            }
        }
    }
}
